#include "../board/sdk_project_config.h"
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>

// 全局变量声明
volatile int g_exitCode = 0;
#define TIMEOUT 500U
#define BUFFER_SIZE 256U
uint8_t g_buffer[BUFFER_SIZE];
uint8_t g_bufferldx;
#define INST_UART_SAMPLE INST_UART_1

// WiFi相关定义
#define TRANSFER_TIMEOUT 200UL
#define RX_BUFFER_SIZE   512U
static superio_device_state_t g_stSuperIODeviceState0;
static superio_uart_state_t g_stSuperioUartState_0;
static superio_uart_state_t g_stSuperioUartState_1;
static uint8_t rxBuffer[RX_BUFFER_SIZE];
static uint8_t commandBuffer[RX_BUFFER_SIZE];
static uint32_t index_buffer = 0;

// 数据处理相关定义
#define WINDOW_SIZE 4       // 512->128需要4倍下采样
#define DATA_INPUT_SIZE 512 // 输入数据量
#define DATA_OUTPUT_SIZE 128// 输出数据量

// 数据处理器结构体
typedef struct {
    float window[WINDOW_SIZE];
    int window_idx;
    float processed_data[DATA_OUTPUT_SIZE];
    int processed_idx;
} DataProcessor;

// 串口接收缓冲区
typedef struct {
    uint8_t buf[4096];  // 足够存储512个数字的字符串
    int len;
    bool frame_complete;
} SerialRxBuffer;

SerialRxBuffer serial_rx_buf = {0};
DataProcessor data_processor = {0};

// 函数声明
void RxCallback(void *driverState, uart_event_t event, void *userData);
bool connectWiFi();
bool createTCPConnection();
bool WiFi_send(const char* data);
void WiFi_receive(char* recvCmd);
void DataProcessor_Init(DataProcessor *processor);
float mean_denoise(DataProcessor *processor, float new_data);
int downsample(DataProcessor *processor, float denoised_data);
int process_serial_data(DataProcessor *processor, float raw_data);
bool parse_serial_data(uint8_t *buf, int len, float *data);
void pack_processed_data(float *data, uint8_t *buf, int *len);
void handle_control_command(char *cmd);

// 串口接收回调函数
void RxCallback(void *driverState, uart_event_t event, void *userData) {
    (void)driverState;
    (void)userData;

    if (event == UART_EVENT_RX_FULL) {
        // 检查帧结束符
        if (g_buffer[g_bufferldx] == ';' && serial_rx_buf.len > 0) {
            serial_rx_buf.buf[serial_rx_buf.len++] = g_buffer[g_bufferldx];
            serial_rx_buf.frame_complete = true;
            g_bufferldx = 0;
        }
        // 检查帧起始符
        else if (g_buffer[g_bufferldx] == '#' && serial_rx_buf.len == 0) {
            serial_rx_buf.buf[serial_rx_buf.len++] = g_buffer[g_bufferldx];
            g_bufferldx++;
            UART_DRV_SetRxBuffer(INST_UART_SAMPLE, &g_buffer[g_bufferldx], 1U);
        }
        // 正常接收数据
        else if (serial_rx_buf.len > 0 && serial_rx_buf.len < sizeof(serial_rx_buf.buf) - 1) {
            serial_rx_buf.buf[serial_rx_buf.len++] = g_buffer[g_bufferldx];
            g_bufferldx++;
            UART_DRV_SetRxBuffer(INST_UART_SAMPLE, &g_buffer[g_bufferldx], 1U);
        }
        // 缓冲区溢出重置
        else {
            serial_rx_buf.len = 0;
            g_bufferldx = 0;
            UART_DRV_SetRxBuffer(INST_UART_SAMPLE, &g_buffer[g_bufferldx], 1U);
        }
    }
}

// 数据处理器初始化
void DataProcessor_Init(DataProcessor *processor) {
    memset(processor->window, 0, sizeof(processor->window));
    processor->window_idx = 0;
    processor->processed_idx = 0;
    memset(processor->processed_data, 0, sizeof(processor->processed_data));
}

// 均值滤波
float mean_denoise(DataProcessor *processor, float new_data) {
    processor->window[processor->window_idx] = new_data;
    processor->window_idx = (processor->window_idx + 1) % WINDOW_SIZE;

    float sum = 0.0f;
    for (int i = 0; i < WINDOW_SIZE; i++) {
        sum += processor->window[i];
    }
    return sum / WINDOW_SIZE;
}

// 下采样（每4个取一个滤波后的值）
int downsample(DataProcessor *processor, float denoised_data) {
    if (processor->processed_idx >= DATA_OUTPUT_SIZE) {
        return -1;
    }

    if (processor->window_idx % WINDOW_SIZE == 0) {
        processor->processed_data[processor->processed_idx++] = denoised_data;
        return 1;
    }
    return 0;
}

// 处理单条数据
int process_serial_data(DataProcessor *processor, float raw_data) {
    float denoised = mean_denoise(processor, raw_data);
    return downsample(processor, denoised);
}

// 解析串口接收的数据
bool parse_serial_data(uint8_t *buf, int len, float *data) {
    if (len == 0 || buf[0] != '#' || buf[len-1] != ';') {
        return false;
    }

    char *token = strtok((char*)&buf[1], ",;");
    int count = 0;

    while (token != NULL && count < DATA_INPUT_SIZE) {
        data[count++] = atof(token);
        token = strtok(NULL, ",;");
    }

    return count == DATA_INPUT_SIZE;
}

// 打包处理后的数据为响应帧
void pack_processed_data(float *data, uint8_t *buf, int *len) {
    *len = sprintf((char*)buf, "@");
    for (int i = 0; i < DATA_OUTPUT_SIZE; i++) {
        *len += sprintf((char*)&buf[*len], "%.2f", data[i]);
        if (i < DATA_OUTPUT_SIZE - 1) {
            *len += sprintf((char*)&buf[*len], ",");
        }
    }
    *len += sprintf((char*)&buf[*len], ";\r\n");
}

// 处理控制指令
void handle_control_command(char *cmd) {
    int code = -1;
    if (sscanf(cmd, "!cmd=%d;", &code) == 1) {
        // 验证指令有效性
        if (code >= 0 && code <= 5) {
            char ack[32];
            sprintf(ack, "!ack=%d;\r\n", code);
            WiFi_send(ack);  // 发送确认

            // 这里可以添加实际控制指令的执行代码
            switch(code) {
                case 0: printf("执行跳跃指令\r\n"); break;
                case 1: printf("执行前进指令\r\n"); break;
                case 2: printf("执行后退指令\r\n"); break;
                case 3: printf("执行转圈指令\r\n"); break;
                case 4: printf("执行趴下指令\r\n"); break;
                case 5: printf("执行站立指令\r\n"); break;
            }
        }
    }
}

// WiFi相关函数（复用原有代码）
static void ZeroBuffer(uint8_t *buff, uint32_t size) {
    for (uint32_t i = 0; i < size; i++)
        buff[i] = 0;
}

void GetCommandBuffer(uint8_t *rbuffer, uint8_t *cbuffer) {
    int i;
    for (i = index_buffer; i < RX_BUFFER_SIZE; i++) {
        if ((rbuffer[i] == 0xd) & (rbuffer[i+1] == 0xa) & (rbuffer[i+2] == 0x0) & (rbuffer[i+3] == 0x0)) {
            break;
        } else {
            cbuffer[i - index_buffer] = rbuffer[i];
        }
    }
    index_buffer = i + 2;
}

bool JudgeCommandBuffer(uint8_t *cbuffer) {
    bool res = false;
    for (int j = 0; j < RX_BUFFER_SIZE; j++) {
        if ((cbuffer[j] == 0x4f) & (cbuffer[j+1] == 0x4b)) {
            res = true;
            break;
        }
    }
    return res;
}

bool SUPERIO_UartDriverSend_WifiCommand(const char* wifi_com, uint32_t time) {
    ZeroBuffer(rxBuffer, RX_BUFFER_SIZE);
    ZeroBuffer(commandBuffer, RX_BUFFER_SIZE);

    SUPERIO_UART_DRV_ReceiveData(&g_stSuperioUartState_0, rxBuffer, RX_BUFFER_SIZE);
    int len = strlen(wifi_com);
    uint8_t wifi_arr[len+1];
    strcpy((char *)wifi_arr, wifi_com);
    SUPERIO_UART_DRV_SendDataBlocking(&g_stSuperioUartState_1, wifi_arr, len, TRANSFER_TIMEOUT);
    OS_DelayMs(time);

    GetCommandBuffer(rxBuffer, commandBuffer);
    return JudgeCommandBuffer(commandBuffer);
}

// WiFi配置（替换为实际信息）
#define AT_CMD      "AT\r\n"
#define WIFI_SSID "dog"
#define WIFI_PASS "12345678"
#define SET_MODE_CMD "AT+CWMODE=1\r\n"
#define CONNECT_AP_CMD "AT+CWJAP=\""WIFI_SSID"\",\""WIFI_PASS"\"\r\n"

bool connectWiFi() {
    if (!SUPERIO_UartDriverSend_WifiCommand(AT_CMD, 20)) {
        printf("AT命令测试失败\r\n");
        return false;
    }
    if (!SUPERIO_UartDriverSend_WifiCommand(SET_MODE_CMD, 100)) {
        printf("设置Station模式失败\r\n");
        return false;
    }
    if (!SUPERIO_UartDriverSend_WifiCommand(CONNECT_AP_CMD, 20000)) {
        printf("连接AP失败\r\n");
        return false;
    }
    printf("WiFi连接成功\r\n");
    return true;
}

#define SERVER_IP "192.168.12.12"
#define SERVER_PORT 8080
#define CREATE_TCP_CMD "AT+CIPSTART=\"TCP\",\"192.168.12.12\",8080\r\n"
#define SEND_DATA_CMD "AT+CIPSEND=%d\r\n"

bool createTCPConnection() {
    if (!SUPERIO_UartDriverSend_WifiCommand(CREATE_TCP_CMD, 1000)) {
        printf("TCP连接失败\r\n");
        return false;
    }
    printf("TCP连接成功\r\n");
    return true;
}

bool WiFi_send(const char* data) {
    int len = strlen(data);
    char sendCmd[64];
    sprintf(sendCmd, SEND_DATA_CMD, len);
    if (!SUPERIO_UartDriverSend_WifiCommand(sendCmd, 100)) {
        return false;
    }
    SUPERIO_UART_DRV_SendDataBlocking(&g_stSuperioUartState_1, (uint8_t*)data, len, TRANSFER_TIMEOUT);
    OS_DelayMs(100);
    return strstr((char*)commandBuffer, "SEND OK") != NULL;
}

void WiFi_receive(char* recvCmd) {
    SUPERIO_UartDriverSend_WifiCommand("AT+CIPRECVTYPE=1\r\n", 100);
    SUPERIO_UartDriverSend_WifiCommand("AT+CIPRECVLEN?\r\n", 100);
    int dataLen = 0;
    sscanf((char*)commandBuffer, "+CIPRECVLEN:%d", &dataLen);
    if (dataLen <= 0) {
        printf("无数据可接收\r\n");
        return;
    }
    sprintf(recvCmd, "AT+CIPRECVDATA=%d\r\n", dataLen);
    SUPERIO_UartDriverSend_WifiCommand(recvCmd, 100);
}

int main(void)
{
    status_t status;
    uint32_t bytesRemaining;
    float input_data[DATA_INPUT_SIZE];
    uint8_t output_buf[2048];
    int output_len;
    char wifi_recv_cmd[64];

    // 初始化时钟和引脚
    CLOCK_SYS_Init(g_pstClockManConfigsArr[0]);
    PINS_DRV_Init(NUM_OF_CONFIGURED_PINS, g_stPinmuxConfigArr);

    // 初始化UART
    UART_DRV_Init(INST_UART_SAMPLE, &g_stUartState_1, &g_stUart1UserConfig0);
    UART_DRV_InstallRxCallback(INST_UART_SAMPLE, RxCallback, NULL);
    UART_DRV_Init(INST_UART_2, &g_stUartState_2, &g_stUart2UserConfig0);

    // 初始化SuperIO和WiFi UART
    SUPERIO_DRV_InitDevice(INST_SUPERIO_UART_0, &g_stSuperIODeviceState0);
    SUPERIO_UART_DRV_Init(INST_SUPERIO_UART_0, &g_stSuperioUart0UserConfig0, &g_stSuperioUartState_0);
    SUPERIO_UART_DRV_Init(INST_SUPERIO_UART_0, &g_stSuperioUart0UserConfig1, &g_stSuperioUartState_1);

    // 初始化数据处理器
    DataProcessor_Init(&data_processor);

    // 启动UART接收
    g_bufferldx = 0;
    UART_DRV_SetRxBuffer(INST_UART_SAMPLE, &g_buffer[g_bufferldx], 1U);

    // 连接WiFi和TCP服务器
    if (!connectWiFi() || !createTCPConnection()) {
        while (1);
    }

    while (1) {
        // 处理串口接收数据
        if (serial_rx_buf.frame_complete) {
            // 解析接收的数据
            if (parse_serial_data(serial_rx_buf.buf, serial_rx_buf.len, input_data)) {
                // 处理数据（滤波+下采样）
                DataProcessor_Init(&data_processor);
                for (int i = 0; i < DATA_INPUT_SIZE; i++) {
                    process_serial_data(&data_processor, input_data[i]);
                }

                // 打包并发送处理后的数据
                pack_processed_data(data_processor.processed_data, output_buf, &output_len);
                UART_DRV_SendDataBlocking(INST_UART_SAMPLE, output_buf, output_len, TIMEOUT);
            }

            // 重置接收缓冲区
            serial_rx_buf.len = 0;
            serial_rx_buf.frame_complete = false;
            memset(serial_rx_buf.buf, 0, sizeof(serial_rx_buf.buf));
        }

        // 处理WiFi接收的控制指令
        WiFi_receive(wifi_recv_cmd);
        if (strlen((char*)commandBuffer) > 0) {
            handle_control_command((char*)commandBuffer);
            ZeroBuffer(commandBuffer, RX_BUFFER_SIZE);
        }

        OS_DelayMs(10);
    }

    return g_exitCode;
}
